
// ************************************************************************************************************
// board orientation and setup
// ************************************************************************************************************
//default board orientation
#if !defined(ACC_ORIENTATION) 
  #define ACC_ORIENTATION(X, Y, Z)  {accADC[ROLL]  = X; accADC[PITCH]  = Y; accADC[YAW]  = Z;}
#endif
#if !defined(GYRO_ORIENTATION) 
  #define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = X; gyroADC[PITCH] = Y; gyroADC[YAW] = Z;}
#endif
#if !defined(MAG_ORIENTATION) 
  #define MAG_ORIENTATION(X, Y, Z)  {magADC[ROLL]  = X; magADC[PITCH]  = Y; magADC[YAW]  = Z;}
#endif

//MPU6050 Gyro LPF setting
#if defined(MPU6050_LPF_256HZ) || defined(MPU6050_LPF_188HZ) || defined(MPU6050_LPF_98HZ) || defined(MPU6050_LPF_42HZ) || defined(MPU6050_LPF_20HZ) || defined(MPU6050_LPF_10HZ)
  #if defined(MPU6050_LPF_256HZ)
    #define MPU6050_SMPLRT_DIV 0  //8000Hz
    #define MPU6050_DLPF_CFG   0
  #endif
  #if defined(MPU6050_LPF_188HZ)
    #define MPU6050_SMPLRT_DIV 0  //1000Hz
    #define MPU6050_DLPF_CFG   1
  #endif
  #if defined(MPU6050_LPF_98HZ)
    #define MPU6050_SMPLRT_DIV 0
    #define MPU6050_DLPF_CFG   2
  #endif
  #if defined(MPU6050_LPF_42HZ)
    #define MPU6050_SMPLRT_DIV 0
    #define MPU6050_DLPF_CFG   3
  #endif
  #if defined(MPU6050_LPF_20HZ)
    #define MPU6050_SMPLRT_DIV 0
    #define MPU6050_DLPF_CFG   4
  #endif
  #if defined(MPU6050_LPF_10HZ)
    #define MPU6050_SMPLRT_DIV 0
    #define MPU6050_DLPF_CFG   5
  #endif
#else
    //Default settings LPF 256Hz/8000Hz sample
    #define MPU6050_SMPLRT_DIV 0  //8000Hz
    #define MPU6050_DLPF_CFG   0
#endif

uint8_t rawADC[6];
static uint32_t neutralizeTime = 0;
  
// ************************************************************************************************************
// I2C general functions
// ************************************************************************************************************
 uint8 i2c_readReg(uint8 add,uint8 reg)
 {
   uint8 result[1];
  #if 1 
   if(Wire.readMultiByte(add,reg,result,1)!=SUCCESS)return 0xfe;
   #else
    Wire.beginTransmission(add);
   Wire.send(reg); 
   Wire.endTransmission(); //end transmission
   if( Wire.readMultiByte(add,result,1)!=SUCCESS)return 0xfE;
    #endif
   return result[0];
 }
void i2c_writeReg(uint8_t add, uint8_t reg, uint8_t val) 
{
   
 Wire.beginTransmission(add);
  Wire.send(reg); 
  Wire.send(val); 
  Wire.endTransmission(); //end transmission
  
  
}
void i2c_getSixRawADC(uint8_t add, uint8_t reg) {
   //soft I2C
   #if 1
   if(Wire.readMultiByte(add,reg,rawADC,6)!=SUCCESS)
   {
        neutralizeTime = micros(); //we take a timestamp here to neutralize the WMP or WMP+NK values during a short delay after the hard reset
    }
   #else
   Wire.beginTransmission(add);
   Wire.send(reg); 
   Wire.endTransmission(); //end transmission
 
   if(Wire.readMultiByte(add,rawADC, 6)!=SUCCESS)    // request 6 bytes from device=0;
   {
     #ifndef ITG3200
      WMP_init(0);
        #endif
     neutralizeTime = micros(); //we take a timestamp here to neutralize the WMP or WMP+NK values during a short delay after the hard reset

   }
  #endif
}
 
 
#define MPU6050_ACC
//#define MMA8451Q
// ************************************************************************************************************
// I2C Accelerometer MMA8451Q 
// ************************************************************************************************************
#if defined(MMA8451Q)

#define MMA8452_ADDRESS     0x1C

#define MMA8452_DEVICE_SIGNATURE    0x2A
#define MMA8451_DEVICE_SIGNATURE    0x1A

#define MMA8452_STATUS              0x00
#define MMA8452_OUT_X_MSB           0x01
#define MMA8452_WHO_AM_I            0x0D
#define MMA8452_XYZ_DATA_CFG        0x0E
#define MMA8452_HP_FILTER_CUTOFF    0x0F
#define MMA8452_CTRL_REG1           0x2A
#define MMA8452_CTRL_REG2           0x2B
#define MMA8452_CTRL_REG3           0x2C
#define MMA8452_CTRL_REG4           0x2D
#define MMA8452_CTRL_REG5           0x2E

#define MMA8452_FS_RANGE_8G         0x02
#define MMA8452_FS_RANGE_4G         0x01
#define MMA8452_FS_RANGE_2G         0x00

#define MMA8452_HPF_CUTOFF_LV1      0x00
#define MMA8452_HPF_CUTOFF_LV2      0x01
#define MMA8452_HPF_CUTOFF_LV3      0x02
#define MMA8452_HPF_CUTOFF_LV4      0x03

#define MMA8452_CTRL_REG2_B7_ST     0x80
#define MMA8452_CTRL_REG2_B6_RST    0x40
#define MMA8452_CTRL_REG2_B4_SMODS1 0x10
#define MMA8452_CTRL_REG2_B3_SMODS0 0x08
#define MMA8452_CTRL_REG2_B2_SLPE   0x04
#define MMA8452_CTRL_REG2_B1_MODS1  0x02
#define MMA8452_CTRL_REG2_B0_MODS0  0x01

#define MMA8452_CTRL_REG2_MODS_LP   0x03
#define MMA8452_CTRL_REG2_MODS_HR   0x02
#define MMA8452_CTRL_REG2_MODS_LNLP 0x01
#define MMA8452_CTRL_REG2_MODS_NOR  0x00

#define MMA8452_CTRL_REG3_IPOL          0x02
#define MMA8452_CTRL_REG4_INT_EN_DRDY   0x01

#define MMA8452_CTRL_REG1_LNOISE        0x04
#define MMA8452_CTRL_REG1_ACTIVE        0x01

void ACC_init () {
  delay(10);
  //i2c_writeReg(MMA8451Q_ADDRESS,0x2A,0x05); // wake up & low noise
 // delay(10);
  //i2c_writeReg(MMA8451Q_ADDRESS,0x0E,0x02); // full scale range
  
   i2c_writeReg(MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff
    i2c_writeReg(MMA8452_ADDRESS, MMA8452_XYZ_DATA_CFG, MMA8452_FS_RANGE_8G);
    i2c_writeReg(MMA8452_ADDRESS, MMA8452_HP_FILTER_CUTOFF, MMA8452_HPF_CUTOFF_LV4);
    i2c_writeReg(MMA8452_ADDRESS, MMA8452_CTRL_REG2, MMA8452_CTRL_REG2_MODS_HR | MMA8452_CTRL_REG2_MODS_HR << 3); // High resolution measurement in both sleep and active modes
    i2c_writeReg(MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)
    i2c_writeReg(MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver)
    i2c_writeReg(MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
    i2c_writeReg(MMA8452_ADDRESS, MMA8452_CTRL_REG1, MMA8452_CTRL_REG1_LNOISE | MMA8452_CTRL_REG1_ACTIVE); // Turn on measurements, low noise at max scale mode, Data Rate 800Hz. LNoise mode makes range +-4G.

  acc_1G = 256;
}

void ACC_getADC () {
   int16 tmpADC;
  i2c_getSixRawADC(MMA8452_ADDRESS,0x01);

   tmpADC=(int16)((rawADC[2]<<8) | rawADC[3])>>2;
   accADC[ROLL]  = - ( tmpADC/4);
   tmpADC=(int16)((rawADC[0]<<8) | rawADC[1])>>2;
   accADC[PITCH]  = +( tmpADC /4);
   tmpADC=(int16)((rawADC[4]<<8) | rawADC[5])>>2;
   accADC[YAW]  =  + ( tmpADC/4 ); 		
   
  
  ACC_Common();
}
#endif
// ************************************************************************************************************
// I2C Gyroscope and Accelerometer MPU6050
// ************************************************************************************************************
#if defined(MPU6050)
#define MPU6050_ADDRESS     0x68 // address pin AD0 low (GND), default for FreeIMU v0.4 and InvenSense evaluation board
void Gyro_init() {
   int16 tmpVal;
  i2c_writeReg(MPU6050_ADDRESS, 0x6B, 0x80);             //PWR_MGMT_1    -- DEVICE_RESET 1
  delay(5);
  i2c_writeReg(MPU6050_ADDRESS, 0x19, 0x00);             //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
  
  i2c_writeReg(MPU6050_ADDRESS, 0x6B, 0x03);             //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
  i2c_writeReg(MPU6050_ADDRESS, 0x1B, 0x18);             //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
  // enable I2C bypass for AUX I2C
  #if defined(MAG)
    i2c_writeReg(MPU6050_ADDRESS, 0x6A, 0x00);             //USER_CTRL     -- DMP_EN=0 ; FIFO_EN=0 ; I2C_MST_EN=0 (I2C bypass mode) ; I2C_IF_DIS=0 ; FIFO_RESET=0 ; I2C_MST_RESET=0 ; SIG_COND_RESET=0
    i2c_writeReg(MPU6050_ADDRESS, 0x37, 0x02);             //INT_PIN_CFG   -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=1 ; CLKOUT_EN=0
  #endif
  i2c_writeReg(MPU6050_ADDRESS, 0x1A, MPU6050_DLPF_CFG); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
}

void Gyro_getADC () {
	int16 tmpVal;
	int16 tmpADCx,tmpADCy;
  i2c_getSixRawADC(MPU6050_ADDRESS, 0x43);
 #if 1 
   tmpVal=(rawADC[0]<<8) | rawADC[1];
  gyroADC[ROLL]  =  + ( tmpVal/4 );
  
   tmpVal=(rawADC[2]<<8) | rawADC[3];   
  gyroADC[PITCH] = +( tmpVal/4 );
  
   tmpVal=(rawADC[4]<<8) | rawADC[5];
  gyroADC[YAW]   =  -( tmpVal/4 );
  #else //for quadx
   tmpADCx=(int16)((rawADC[0]<<8) | rawADC[1])/4;
   tmpADCy=(int16)((rawADC[2]<<8) | rawADC[3])/4;
   
   gyroADC[ROLL]  = (int16)((float)(tmpADCx+tmpADCy)*0.7071);
   gyroADC[PITCH]  =(int16)((float)(tmpADCy-tmpADCx)*0.7071);    
   
   tmpVal=(int16)((rawADC[4]<<8) | rawADC[5]);
    gyroADC[YAW]   =  -( tmpVal/4 );	
#endif

  GYRO_Common();
}
#if defined(MPU6050_ACC)
void ACC_init () {
  i2c_writeReg(MPU6050_ADDRESS, 0x1C, 0x10);             //ACCEL_CONFIG  -- AFS_SEL=2 (Full Scale = +/-8G)  ; ACCELL_HPF=0   //note something is wrong in the spec.
  //note: something seems to be wrong in the spec here. With AFS=2 1G = 4096 but according to my measurement: 1G=2048 (and 2048/8 = 256)
  //confirmed here: http://www.multiwii.com/forum/viewtopic.php?f=8&t=1080&start=10#p7480
   
    acc_1G = 512;

}

void ACC_getADC () {
 int16 tmpADC;
 int16 tmpADCx,tmpADCy;
  i2c_getSixRawADC(MPU6050_ADDRESS, 0x3B);
  #if 1
 tmpADC=(int16)((rawADC[2]<<8) | rawADC[3]);
   accADC[ROLL]  = + ( tmpADC/8 );
   
   tmpADC=(int16)((rawADC[0]<<8) | rawADC[1]);
   accADC[PITCH]  = -( tmpADC /8);
   
   tmpADC=(int16)((rawADC[4]<<8) | rawADC[5]);
   accADC[YAW]  =  + ( tmpADC/8 ); 		
#else //for quadx
   tmpADCx=(int16)((rawADC[2]<<8) | rawADC[3])/8;
   tmpADCy=(int16)((rawADC[0]<<8) | rawADC[1])/8;
   
   accADC[ROLL]  = (int16)((float)(tmpADCx+tmpADCy)*0.7071);
   accADC[PITCH]  =(int16)((float)(-tmpADCy-tmpADCx)*0.7071);    
   
   tmpADC=(int16)((rawADC[4]<<8) | rawADC[5]);
   accADC[YAW]  =  + ( tmpADC/8 ); 		
#endif
   
  ACC_Common();
}
#endif
#endif
// ****************
// GYRO common part
// ****************
void GYRO_Common() {
  static int16_t previousGyroADC[3] = {0,0,0};
  static int32_t g[3];
  uint8_t axis;
  
  if (calibratingG>0) {
    for (axis = 0; axis < 3; axis++) {
      // Reset g[axis] at start of calibration
        if (calibratingG == 400) g[axis]=0;
      // Sum up 400 readings
        g[axis] +=gyroADC[axis];
      // Clear global variables for next reading
        gyroADC[axis]=0;
        gyroZero[axis]=0;
      if (calibratingG == 1) {
        gyroZero[axis]=g[axis]/400;
        blinkLED(10,15,1+3*nunchuk);
      }
    }
    calibratingG--;
  }
  //anti gyro glitch, limit the variation between two consecutive readings
  for (axis = 0; axis < 3; axis++) {
    gyroADC[axis]  -= gyroZero[axis];
    gyroADC[axis] = constrain(gyroADC[axis],previousGyroADC[axis]-800,previousGyroADC[axis]+800);
    previousGyroADC[axis] = gyroADC[axis];
  }
}


// ****************
// ACC common part
// ****************
void ACC_Common() {
  static int32_t a[3];
  
  if (calibratingA>0) {
      for (uint8_t axis = 0; axis < 3; axis++) {
      // Reset a[axis] at start of calibration
        if (calibratingA == 400) a[axis]=0;
      // Sum up 400 readings
        a[axis] +=accADC[axis];
      // Clear global variables for next reading
        accADC[axis]=0;
        accZero[axis]=0;
      }
    // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
    if (calibratingA == 1) {
      accZero[ROLL]  = a[ROLL]/400;
      accZero[PITCH] = a[PITCH]/400;
      accZero[YAW]   = a[YAW]/400-acc_1G; // for nunchuk 200=1G
      accTrim[ROLL]   = 0;
      accTrim[PITCH]  = 0;

      writeParams(); // write accZero in EEPROM
    }
    calibratingA--;
  }
  accADC[ROLL]  -=  accZero[ROLL] ;
  accADC[PITCH] -=  accZero[PITCH];
  accADC[YAW]   -=  accZero[YAW] ;
}

  
#if MAG
void Mag_getADC() {
  static uint32_t t,tCal = 0;
  static int16_t magZeroTempMin[3];
  static int16_t magZeroTempMax[3];
  uint8_t axis;
  if ( (micros()-t )  < 100000 ) return; //each read is spaced by 100ms
  t = micros();
  
  Device_Mag_getADC();
  if (calibratingM == 1) {
    tCal = t;
    for(axis=0;axis<3;axis++) {magZero[axis] = 0;magZeroTempMin[axis] = 0; magZeroTempMax[axis] = 0;}
    calibratingM = 0;
  }
  magADC[ROLL]  -= magZero[ROLL];
  magADC[PITCH] -= magZero[PITCH];
  magADC[YAW]   -= magZero[YAW];
  if (tCal != 0) {
    if ((t - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
      LEDPIN_SWITCH
      for(axis=0;axis<3;axis++) {
        if (magADC[axis] < magZeroTempMin[axis]) magZeroTempMin[axis] = magADC[axis];
        if (magADC[axis] > magZeroTempMax[axis]) magZeroTempMax[axis] = magADC[axis];
      }
    } else {
      tCal = 0;
      for(axis=0;axis<3;axis++)
        magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis])/2;
      writeParams();
    }
  }
}
#endif
#ifdef LSM303DLHC

void Mag_init(void)
{
  delay(100);
  i2c_writeReg( LSM303DLHC_M_ADDR  ,0x00 ,(0x05<<2) ); // CRA_REG_M data out rate 30hz
  delay(5);
   
  i2c_writeReg( LSM303DLHC_M_ADDR  ,0x01,(0x05<<5)); // CRB_REG_m 4.7
  delay(5);
  i2c_writeReg( LSM303DLHC_M_ADDR  ,0x02,0X00);//MR_REG_M  Continuous-conversion mode
}
void Device_Mag_getADC() 
{ 
  int16 tmpADC;
  i2c_getSixRawADC(LSM303DLHC_M_ADDR,0x80|0x03);   
   tmpADC=(int16)((rawADC[0]<<8) | rawADC[1]);
   magADC[ROLL]  = + ( tmpADC/16 );
   tmpADC=(rawADC[2]<<8) | rawADC[3];
   magADC[PITCH]  = -( tmpADC /16);
   tmpADC=(rawADC[4]<<8) | rawADC[5];
   magADC[YAW]  =  + ( tmpADC/16 );
}
void ACC_init(void)
{
  delay(100);
  i2c_writeReg( LSM303DLHC_A_ADDR  ,0x20 ,0x77 ); // CTRL_REG1  High 4bit 1:1hz 2:10hz 3:25hz 4:50 5:100 6: 200hz 7:400hz 8:1.62khz 9:1.344khz
  delay(5);
   
 // i2c_writeReg( LSM303DLHC_A_ADDR  ,0x21 ,0x40|0x03|0x0c ); // CTRL_REG2 
   delay(5);  
   i2c_writeReg( LSM303DLHC_A_ADDR  ,0x23 ,0x20|0x08 ); // CTRL_REG4 8g 
   
   
   acc_1G = 250;
    
}
void ACC_getADC()
{
  int16 tmpADC;
  
   i2c_getSixRawADC(LSM303DLHC_A_ADDR,0x80|0x28);
   tmpADC=(int16)((rawADC[3]<<8) | rawADC[2]);
   accADC[ROLL]  = + ( tmpADC/16 );
   tmpADC=(int16)((rawADC[1]<<8) | rawADC[0]);
   accADC[PITCH]  = -( tmpADC /16);
   tmpADC=(int16)((rawADC[5]<<8) | rawADC[4]);
   accADC[YAW]  =  + ( tmpADC/16 ); 
  ACC_Common();
}
#endif

// ************************************************************************************************************
// I2C Barometer MS561101BA
// ************************************************************************************************************
#if !defined(MS561101BA_ADDRESS) 
  #define MS561101BA_ADDRESS 0x77 //CBR=0 0xEE I2C address when pin CSB is connected to LOW (GND)
  //#define MS561101BA_ADDRESS 0x76 //CBR=1 0xEC I2C address when pin CSB is connected to HIGH (VCC)
#endif
// specs are here: http://www.meas-spec.com/downloads/MS5611-01BA03.pdf
// useful info on pages 7 -> 12
#if defined(MS561101BA)

// registers of the device
#define MS561101BA_PRESSURE    0x40
#define MS561101BA_TEMPERATURE 0x50
#define MS561101BA_RESET       0x1E

// OSR (Over Sampling Ratio) constants
#define MS561101BA_OSR_256  0x00
#define MS561101BA_OSR_512  0x02
#define MS561101BA_OSR_1024 0x04
#define MS561101BA_OSR_2048 0x06
#define MS561101BA_OSR_4096 0x08

#define OSR MS561101BA_OSR_4096

static struct {
  // sensor registers from the MS561101BA datasheet
  uint16_t c[7];
  union {uint32_t val; uint8_t raw[4]; } ut; //uncompensated T
  union {uint32_t val; uint8_t raw[4]; } up; //uncompensated P
  uint8_t  state;
  uint32_t deadline;
} ms561101ba_ctx;

void i2c_MS561101BA_reset(){
  i2c_writeReg(MS561101BA_ADDRESS, MS561101BA_RESET, 0);
}

void i2c_MS561101BA_readCalibration(){
  union {uint16_t val; uint8_t raw[2]; } data;
  uint8_t rawData[2];
  
  for(uint8_t i=0;i<6;i++) 
  {
  	 Wire.readMultiByte(MS561101BA_ADDRESS,0xA2+2*i,rawData,2);
     
    data.raw[1] = rawData[0];  // read a 16 bit register
    data.raw[0] = rawData[1];
    ms561101ba_ctx.c[i+1] = data.val;
  }
}

void  Baro_init() {
  delay(10);
  i2c_MS561101BA_reset();
  delay(100);
  i2c_MS561101BA_readCalibration();
 delay(10);
  //i2c_MS561101BA_UT_Start(); 
  //ms561101ba_ctx.deadline = currentTime+10000;
}

// read uncompensated temperature value: send command first
void i2c_MS561101BA_UT_Start() {
    
  Wire.beginTransmission(MS561101BA_ADDRESS);
  Wire.send(MS561101BA_TEMPERATURE + OSR);   
  Wire.endTransmission(); //end transmission
   
}

// read uncompensated pressure value: send command first
void i2c_MS561101BA_UP_Start () {
	
  Wire.beginTransmission(MS561101BA_ADDRESS);
  Wire.send(MS561101BA_PRESSURE + OSR);   
  Wire.endTransmission(); //end transmission
   
}

// read uncompensated pressure value: read result bytes
void i2c_MS561101BA_UP_Read () {
	uint8_t rawData[3];
	Wire.readMultiByte(MS561101BA_ADDRESS,0,rawData,3);
  
  ms561101ba_ctx.up.raw[2] = rawData[0];
  ms561101ba_ctx.up.raw[1] = rawData[1];
  ms561101ba_ctx.up.raw[0] = rawData[2];
}

// read uncompensated temperature value: read result bytes
void i2c_MS561101BA_UT_Read() {
  uint8_t rawData[3];
  //Wire.readMultiByte(MS561101BA_ADDRESS,0,rawData,3);
  Wire.readMultiByte(MS561101BA_ADDRESS,0,rawData,3);
  //SerialUSB.print("\r\RAW:");SerialUSB.print(rawData[0],16);SerialUSB.print(rawData[1],16);SerialUSB.print(rawData[2],16);
  ms561101ba_ctx.ut.raw[2] = rawData[0];
  ms561101ba_ctx.ut.raw[1] = rawData[1];
  ms561101ba_ctx.ut.raw[0] = rawData[2];
  //SerialUSB.print("\r\RAW:");SerialUSB.print( ms561101ba_ctx.ut.raw[0],16);SerialUSB.print( ms561101ba_ctx.ut.raw[1],16);SerialUSB.print( ms561101ba_ctx.ut.raw[2],16);
}

// use float approximation instead of int64_t intermediate values
// does not use 2nd order compensation under -15 deg
void i2c_MS561101BA_Calculate() {
 int32_t temperature,off2=0,sens2=0,delt;

  int32_t dT   = ms561101ba_ctx.ut.val - ((uint32_t)ms561101ba_ctx.c[5] << 8);
  int64_t off  = ((uint32_t)ms561101ba_ctx.c[2] <<16) + (((int64_t)dT * ms561101ba_ctx.c[4]) >> 7);
  int64_t sens = ((uint32_t)ms561101ba_ctx.c[1] <<15) + (((int64_t)dT * ms561101ba_ctx.c[3]) >> 8);
  temperature  = 2000 + (((int64_t)dT * ms561101ba_ctx.c[6])>>23);

  if (temperature < 2000) { // temperature lower than 20st.C 
    delt = temperature-2000;
    delt  = delt*delt;
    off2  = (5 * delt)>>1; 
    sens2 = (5 * delt)>>2; 
    if (temperature < -1500) { // temperature lower than -15st.C
      delt  = temperature+1500;
      delt  = delt*delt;
      off2  += 7 * delt; 
      sens2 += (11 * delt)>>1; 
    }
  } 
  off  -= off2; 
  sens -= sens2;
  pressure     = (( (ms561101ba_ctx.up.val * sens ) >> 21) - off) >> 15;

}

//return 0: no data available, no computation ;  1: new value available  ; 2: no new value, but computation time
void Baro_update() {                          // first UT conversion is started in init procedure
 if (currentTime < ms561101ba_ctx.deadline) return; 
   ms561101ba_ctx.deadline = currentTime;
  #if 0
  ms561101ba_ctx.deadline = currentTime+15000;
  if(ms561101ba_ctx.state==0)
  {
    i2c_MS561101BA_UT_Read(); 
    i2c_MS561101BA_UP_Start(); 
    ms561101ba_ctx.state = 1;
  }
  else
  {//toggleLED();
   i2c_MS561101BA_UP_Read();
    i2c_MS561101BA_UT_Start(); 
    i2c_MS561101BA_Calculate();
     BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 44330.0f;
    ms561101ba_ctx.state = 0;  
  }
  //TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, MS5611 is ok with this speed
 #else
  switch (ms561101ba_ctx.state) {
    case 0: 
      i2c_MS561101BA_UT_Start(); 
      ms561101ba_ctx.state++; ms561101ba_ctx.deadline += 10000; //according to the specs, the pause should be at least 8.22ms
      break;
    case 1: 
      i2c_MS561101BA_UT_Read(); 
      ms561101ba_ctx.state++;
      break;
    case 2: 
      i2c_MS561101BA_UP_Start(); 
      ms561101ba_ctx.state++; ms561101ba_ctx.deadline += 10000; //according to the specs, the pause should be at least 8.22ms
      break;
    case 3: 
      i2c_MS561101BA_UP_Read();
      i2c_MS561101BA_Calculate();
      BaroAlt = (1.0f - pow(pressure/101325.0f, 0.190295f)) * 44330.0f;
      ms561101ba_ctx.state = 0; ms561101ba_ctx.deadline += 40000;
       //SerialUSB.print("\r\RAW:");SerialUSB.print(ms561101ba_ctx.up.raw[0],16);SerialUSB.print(ms561101ba_ctx.up.raw[1],16);SerialUSB.print(ms561101ba_ctx.up.raw[2],16);
      break;
      
  } 
  #endif
}
#endif

 


void initSensors() {
    POWERPIN_PINMODE;
    POWERPIN_OFF //switch OFF WMP
    delay(100);  
    POWERPIN_ON  //switch ON WMP
    delay(100);

  delay(100);
  #ifndef HARD_I2C
   Wire.begin(  30, 29);
   #endif
  delay(100);
   
   
   
   if(GYRO) Gyro_init();
   if (BARO) Baro_init();  
   if (MAG) Mag_init();
   if(ACC)ACC_init();
  
}
